import json
import logging
import math
import os
import random
import sys
import uuid
from collections import Counter
from datetime import datetime
from math import sin, asin, cos, radians, sqrt, degrees, atan2
from config.config import USE_LIBSUMO
import sumolib
if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci
import traci.constants as tc
from traci import TraCIException

# 输出异常信息
errorMsg = None
# 保存每次增加车辆的车辆信息
carIAddInfo = {}
# 车辆添加后，车辆不一定在IdList中出现，需要设置变量保存已经在idList中出现的变量
carInIdList = []
oneCarId = None
# 指定速度发车临时变量
carSpeedTemp = {}
carIdTemp = []
plexe = None
population = {1, 3, 15}
car_type_dict = {
    "busWeight": 3,
    "carWeight": 1,
    "truckWeight": 15
}
weights = [(1, 0.7), (3, 0.2), (15, 0.1)]
carMap = {}
AreaList = []
msgTemp = {}

emergency_traffic_list = list()
exclusive_traffic_list = list()
LANE_SPLICER = "_"
JUN_SPLICER = ":"
INTERNAL_SIGN = "internal"

# 全局发车id
vehicle_id = 0
from loguru import logger


def get_downstream_position_simplified(net, edge_id, pos, distance=50):
    """
    简化版：计算沿路网方向下行指定距离后的路段ID和位置，不考虑车道

    参数:
        net: sumolib.net.Net 对象（通过 sumolib.net.readNet() 获取）
        edge_id: 当前 Edge 的 ID
        pos: 当前位置在 Edge 上的纵向距离（米）
        distance: 需要下行的距离（米）

    返回:
        (new_edge_id, new_pos): 新位置的路段ID和纵向距离
    """
    remaining_distance = distance

    while remaining_distance > 0:
        # 获取当前 Edge 对象
        edge = net.getEdge(edge_id)
        if edge is None:
            raise ValueError(f"Edge ID '{edge_id}' 不存在")

        # 获取 Edge 总长度
        edge_length = edge.getLength()
        # logger.info(f"路段{edge_id}长度为{edge_length}")

        # 计算当前 Edge 剩余长度
        remaining_edge_length = edge_length - pos

        if remaining_edge_length >= remaining_distance:
            # 情况1: 剩余距离足够，直接在当前 Edge 上增加 Pos
            new_pos = pos + remaining_distance
            remaining_distance = 0
        else:
            # 情况2: 剩余距离不足，需要切换到下一个 Edge
            remaining_distance -= remaining_edge_length
            new_pos = 0  # 下一个 Edge 的起始位置
            pos = 0
            # 获取当前 Edge 的所有下游连接
            outgoing_connections = edge.getOutgoing()

            if not outgoing_connections:
                # 没有下游连接（可能是路网终点）
                print(f"警告: 已到达路网终点，剩余距离 {remaining_distance} 米未处理")
                break

            # 默认选择第一个下游路段（简化：不考虑车道，只选主路径）
            next_edge = list(outgoing_connections.keys())[0]
            edge_id = next_edge.getID()
            # pos = new_pos  # 重置为新路段的起始位置

    return edge_id, new_pos


def get_downstream_position(edge_id, pos, lane_index, distance):
    """
    计算给定位置沿路网方向下行指定距离后的新位置

    参数:
        edge_id: 当前Edge的ID
        pos: 当前位置在Edge上的纵向距离（米）
        lane_index: 当前车道索引
        distance: 需要下行的距离（米）

    返回:
        (new_edge_id, new_pos, new_lane_index): 新位置的三元组
    """
    remaining_distance = distance
    temp = edge_id
    while remaining_distance > 0:
        # 获取当前车道的ID和长度（修正：通过Lane获取长度）
        lane_id = f"{edge_id}_{lane_index}"
        lane_length = traci.lane.getLength(lane_id)

        # 计算当前Edge剩余长度
        remaining_edge_length = lane_length - pos

        if remaining_edge_length >= remaining_distance:
            # 情况1: 剩余距离足够，直接在当前Edge上增加Pos
            new_pos = pos + remaining_distance
            new_lane_index = lane_index
            remaining_distance = 0
        else:
            # 情况2: 剩余距离不足，需要切换到下一个Edge
            remaining_distance -= remaining_edge_length
            new_pos = 0  # 下一个Edge的起始位置
            pos = 0
            # 获取当前车道的下游连接（修正：使用getLinks()方法）
            links = traci.lane.getLinks(lane_id)

            if not links:
                # 没有下游连接（可能是路网终点）
                print(f"警告: {temp}已到达路网终点，剩余距离 {remaining_distance} 米未处理")
                break

            # 默认选择第一个连接（直走）
            # 实际应用中可能需要根据转向决策选择正确的连接
            next_link = links[0]
            next_lane_id = next_link[0]  # 获取目标车道ID

            # 更新EdgeID、LaneIndex
            edge_id = next_lane_id[:next_lane_id.rfind('_')]  # 提取EdgeID
            lane_index = int(next_lane_id[next_lane_id.rfind('_') + 1:])  # 提取车道索引
    return edge_id, new_pos, lane_index


def add_vehicle(depart: dict, sumo_net):
    """
        添加发车信息
    """
    global vehicle_id, weights
    assert 'point' in depart.keys(), '未配置发车点'
    assert 'count' in depart.keys(), '发车数量为空'
    # 获取发车点位置
    start_edge_id, start_pos, start_lane_index = traci.simulation.convertRoad(depart['point'][0], depart['point'][1],
                                                                              True)
    single_edge = sumo_net.getEdge(start_edge_id)
    car_num = depart['count']
    # 获取各个车型比例
    weight_list = []
    for key, value in depart.items():
        if key in car_type_dict:
            car_type = car_type_dict.get(key)
            if value is not None and car_num != 0:
                weight_list.append((car_type, value / 100))
    car_type_dict1 = {}
    for key, value in depart.items():
        if key in car_type_dict:
            car_type_dict1.setdefault(key, car_type_dict.get(key))
    if single_edge is not None:
        temp = [start_edge_id]
        count = 0
        while single_edge is not None:
            edge_bound = single_edge.getOutgoing()
            edge_ids = list()
            for k, v in edge_bound.items():
                if '-361.0.00' in k.getID() or '-254.0.00' in k.getID() or '371.0.00' in k.getID():
                    continue
                edge_ids.append(k.getID())
            if edge_ids is not None and len(edge_ids) > 0:
                try:
                    id = random.choice(edge_ids)
                    temp.append(id)
                    single_edge = sumo_net.getEdge(id)
                except ValueError:
                    continue
            count += 1
            if len(temp) > 1000 or count > 200 or '-547.0.00' in temp:
                print(temp)
                break
        route_id = str(uuid.uuid1())
        traci.route.add(route_id, temp)
        for i in range(car_num):
            weights = [w[1] for w in weight_list]
            element = random.choices(population=list(car_type_dict1.values()), weights=weights, k=1)[0]
            if vehicle_id < 65330:
                vehicle_id = vehicle_id + 1
            else:
                vehicle_id = 0
            vehID = str(vehicle_id)
            length = traci.lane.getLength(start_edge_id + "_" + str(start_lane_index))
            # 车辆初始位置，随机生成[0, 200]之间的值
            position = random.uniform(0, length)
            # 车辆初始速度，随机生成[5, 15]之间的值
            speed = random.uniform(5, 15)
            if element is not None:
                traci.vehicle.add(vehID=vehID, routeID=route_id, typeID=str(element), depart='0',
                                  departLane='free', departPos=str(position), departSpeed=str(speed))
    else:
        print("Road marking problem1")
        errorMsg = "标注错误，请重新标注"


def addVehicle(vehicleNum, vehiclePoints, sumo_net):
    vehiclePointsobj = json.loads(vehiclePoints)
    vid_count = 0
    for vehiclePoint in vehiclePointsobj:
        lat = float(vehiclePoint["lat"])
        lon = float(vehiclePoint["lon"])
        startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon, lat, True)
        single_edge = sumo_net.getEdge(startedgeID)
        if single_edge is not None:
            temp = [startedgeID]
            count = 0
            while single_edge is not None:
                edge_Bound = single_edge.getOutgoing()
                edgeids = list()
                for k, v in edge_Bound.items():
                    edgeids.append(k.getID())
                if edgeids is not None and len(edgeids) > 0:
                    try:
                        id = random.choice(edgeids)
                        if True:
                            temp.append(id)
                        else:
                            logging.info("Road marking problem1")
                            errorMsg = "标注错误，请重新标注"
                        single_edge = sumo_net.getEdge(id)
                    except ValueError:
                        continue
                    # break
                count += 1
                if len(temp) > 1000 or count > 200 or '-547.0.00' in temp:
                    print(temp)
                    break
            routeid = str(uuid.uuid1())

            traci.route.add(routeid, temp)

            for i in range(vehicleNum):
                element = random.choices(population=list(population), weights=[w[1] for w in weights], k=1)[0]
                vehID = str(uuid.uuid4())
                # print(vehID)
                # speed = traci.vehicletype.getMaxSpeed(str(element))
                # print("speed: ",speed)
                # if vid_count == 0:
                #     # platoonUtils.add_vehicle1(plexe, vehID, startpos, startlaneIndex, 20, "passenger2")
                #     traci.vehicle.add(vehID=vehID, routeID=routeid, typeID=str(element), depart='0',
                #                       departLane=startlaneIndex, departPos=startpos, departSpeed=15)
                #     vid_count += 1
                # else:
                length = traci.lane.getLength(startedgeID + "_" + str(startlaneIndex))
                position = random.uniform(0, length)  # 车辆初始位置，随机生成[0, 200]之间的值
                speed = random.uniform(5, 15)  # 车辆初始速度，随机生成[5, 15]之间的值
                # print(1)
                # platoonUtils.add_vehicle1(plexe, vehID, startpos, startlaneIndex, speed, "passenger2")
                traci.vehicle.add(vehID=vehID, routeID=routeid, typeID=str(element), depart='0',
                                  departLane=startlaneIndex, departPos=str(position), departSpeed=str(speed))
                # traci.vehicletype.setAccel(str(element),0.7)



        else:
            logging.info("Road marking problem1")
            errorMsg = "标注错误，请重新标注"


typeid = '1'


def addEtcVehicle(real_vehicle_queue):
    global typeid
    while not real_vehicle_queue.empty():
        vInfo = real_vehicle_queue.get(block=False)
        print("++++++++++: ", vInfo)
        routeID = str(uuid.uuid1())
        temp = ["-1", "-2"]
        traci.route.add(routeID, temp)

        vehiclePicInfo = vInfo['vehiclePicInfos']
        if (vehiclePicInfo['identifyType'] < 4):
            typeid = '1'
        elif (10 < vehiclePicInfo['identifyType'] < 17):
            typeid = '3'
        else:
            typeid = '15'
        # for vehiclePicInfo in vehiclePicInfos:
        if ('headLaneNum' not in vehiclePicInfo.keys()):
            print(1)
        if (vehiclePicInfo['headLaneNum'] == ''):
            if (vehiclePicInfo['licenseCode'] == ''):
                traci.vehicle.add(vehID=vInfo['recordId'], routeID=routeID, typeID=typeid, depart='0',
                                  departLane='0', departSpeed=str(vehiclePicInfo['vehicleSpeed'] / 3.6))
            else:
                traci.vehicle.add(vehID=vehiclePicInfo['licenseCode'], routeID=routeID, typeID=typeid, depart='0',
                                  departLane='0', departSpeed=str(vehiclePicInfo['vehicleSpeed'] / 3.6))

        else:
            traci.vehicle.add(vehID=vehiclePicInfo['licenseCode'], routeID=routeID, typeID=typeid, depart='0',
                              departLane=vehiclePicInfo['headLaneNum'],
                              departSpeed=str(vehiclePicInfo['vehicleSpeed'] / 3.6))


# 事故区辅助变道车辆
auxiliary_list = {}

in_accident_area_veh_list = []
pelxe_falge = 0


# 判断两个点经纬度距离单位 m
def calculate_distance(jing1, wei1, jing2, wei2):
    lng1, lat1, lng2, lat2 = map(radians, [jing1, wei1, jing2, wei2])  # 经纬度转换成弧度
    dlon = lng2 - lng1
    dlat = lat2 - lat1
    a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2
    distance = 2 * asin(sqrt(a)) * 6371.393 * 1000  # 地球平均半径，6371.393km
    return distance


# 前方超车算法
def ChangingLanesAheadToOvertake(vehicles):
    for vehicle_id in vehicles:
        # 获取当前车辆速度
        speed = traci.vehicle.getSpeed(vehicle_id)

        # 获取当前车辆前面的车辆ID
        leader_id = traci.vehicle.getLeader(vehicle_id)

        # 如果当前车辆有前车
        if leader_id is not None:
            # 获取当前车辆与前车之间的距离
            # vehicle_id_pos=traci.vehicle.getPosition(vehicle_id)
            # leader_id_pos=traci.vehicle.getPosition(leader_id)
            # longitude1, latitude1 = traci.simulation.convertGeo(vehicle_id_pos[0], vehicle_id_pos[1])
            # longitude2, latitude2 = traci.simulation.convertGeo(leader_id_pos[0], leader_id_pos[1])
            # distance=calculate_distance(longitude1,latitude1,longitude2,latitude2)

            distance = leader_id[1]

            # 获取前车速度
            leader_speed = traci.vehicle.getSpeed(leader_id[0])

            # 如果当前车辆速度小于前车速度并且距离小于10米
            if speed > leader_speed and 50 > distance > 15:
                pos = traci.vehicle.getPosition(vehicle_id)
                edgeID, pos, lanindex = traci.simulation.convertRoad(pos[0], pos[1])
                num_lanes = traci.edge.getLaneNumber(edgeID)
                # 尝试向左变道
                # if num_lanes>1:
                #     if lanindex == 0:
                #         traci.vehicle.changeLane(vehicle_id, 1, 10)
                #     elif lanindex == 1:
                #         traci.vehicle.changeLane(vehicle_id, 0, 10)
                #     elif lanindex == 2:
                #         traci.vehicle.changeLane(vehicle_id, 1, 10)
                #     elif lanindex == 3:
                #         traci.vehicle.changeLane(vehicle_id, 2, 10)


# 超车
def Overtaking(tag_vehicle_id, global_vehicles):
    try:
        # 获取当前车辆的位置和速度信息
        # vehicle_pos = traci.vehicle.getPosition(vehicle_id)
        # vehicle_speed = traci.vehicle.getSpeed(vehicle_id)
        # 判断主车前方是否有车
        if "v.0.0" in tag_vehicle_id:
            leader_ids = ['v.0.0', 'v.0.1', 'v.0.2', 'v.0.3']
            surrounding_vehicles = get_surrounding_vehicles(tag_vehicle_id, global_vehicles, 50)
            signals = traci.vehicle.getSignals('v.0.0')
            # print('signals: ', signals)
            for surrounding_vehicle in surrounding_vehicles:
                traci.vehicle.setSignals(surrounding_vehicle, signals)

                if surrounding_vehicle not in leader_ids and ('-284.0.00' in traci.vehicle.getRoadID(
                        surrounding_vehicle) or '-285.0.00' in traci.vehicle.getRoadID(surrounding_vehicle)):
                    traci.vehicle.setSignals(surrounding_vehicle, 8)
                    traci.vehicle.setLaneChangeMode(surrounding_vehicle, 512)
                    # continue
                if surrounding_vehicle in leader_ids and ('-284.0.00' in traci.vehicle.getRoadID(
                        surrounding_vehicle) or '-285.0.00' in traci.vehicle.getRoadID(surrounding_vehicle)):
                    traci.vehicle.setSignals(surrounding_vehicle, 2)
                    traci.vehicle.setLaneChangeMode(surrounding_vehicle, 0)

            # print('surrounding_vehicles: ',surrounding_vehicles)
            # p_leader_id=traci.vehicle.subscribeLeader(vehicle_id)
            # 获取前车的位置和速度信息
            p_leader_id = traci.vehicle.getLeader(vehicle_id)
            # p_leader_id = veh_info[tc.VAR_LEADER]
            if p_leader_id is not None:
                if p_leader_id[1] < 100:
                    # traci.vehicle.setSpeed(vehicle_id,-1)
                    traci.vehicle.changeLaneRelative(p_leader_id[0], -1, 10)
        # # 获取前车的位置和速度信息
        # leader_id = traci.vehicle.getLeader(vehicle_id)
        # if leader_id is not None:
        #     leader_pos = traci.vehicle.getPosition(leader_id[0])
        #     leader_speed = traci.vehicle.getSpeed(leader_id[0])
        #
        #     # 计算两车之间的距离
        #     # distance = math.sqrt((leader_pos[0] - vehicle_pos[0]) ** 2 + (leader_pos[1] - vehicle_pos[1]) ** 2)
        #
        #     # 判断两车之间的距离是否大于150
        #     if leader_id[1] > 150:
        #         traci.vehicle.setSpeed(vehicle_id, leader_speed + 10)  # 加速行驶
        #         traci.vehicle.setAcceleration(vehicle_id, 2, 5)
        #     else:
        #         traci.vehicle.setSpeed(vehicle_id, vehicle_speed)  # 维持当前速度
    except Exception as e:
        logging.info("sumo Error" + str(e))
    #     # clearmessage()


# 判断是否触发场景区域
# 判断车辆到达距离限速点距离长度距离
distance1 = 0
# 创建5个点之间的距离
dists = []
accelerations = []

# a = {}
# a.keys()
# id in a.keys():
# a[id] = dis
distance_dicts = {}
speed_dicts = {}
speed_plexe = 0
leader_ids = ['v.0.0', 'v.0.1', 'v.0.2', 'v.0.3']


def is_point_in_area(vehicles: dict, speed_limit_points: list, speed_speeds: list):
    global distance1, dists, accelerations, distance_dicts, speed_plexe, leader_ids
    # 获取到场景区域内的车辆
    # 对场景区域内的车辆进行限速设置
    array = []
    try:
        for vehicle_id, vehicle_info in vehicles.items():
            # if vehicle_id in leader_ids:
            #     # traci.vehicle.setLaneChangeMode(vehicle_id, 512)
            #     # traci.vehicle.setMinGap(vehicle_id, 5)  # 定义与前车的最小间距
            #     continue
            array.append(vehicle_id)
            # 如果车辆此时没在这个区域内，则可能是刚刚进入这个区域，也可能已经处于这个区域内
            if vehicle_id not in distance_dicts:
                veh_distance = 99999999
                number = 0
                # 此时判断离该车辆最近的经纬度点
                for i in range(len(speed_limit_points)):
                    lat = float(speed_limit_points[i]["lat"])
                    lon = float(speed_limit_points[i]["lon"])
                    veh_pos = vehicle_info[tc.VAR_POSITION]
                    veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                    veh_distance2 = calculate_distance(veh_lon, veh_lat, lon, lat)
                    if veh_distance2 <= veh_distance:
                        number = i
                        veh_distance = veh_distance2
                        distance_dicts[vehicle_id] = [lat, lon, speed_speeds[i]]
                # 计算车辆的加速度
                acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                      float(speed_speeds[number]) / 3.6, veh_distance)
                t = calc_time_to_reach2(float(speed_speeds[number]) / 3.6, str(veh_distance),
                                        vehicle_info[tc.VAR_SPEED], acceleration)
                traci.vehicle.slowDown(vehicle_id, float(speed_speeds[number]) / 3.6, t)
                # traci.vehicle.setSpeed(vehicle_id, float(HttpInterface.speeds[number])/3.6)
            else:
                # 首先判断车辆是否已经经过了这个点位（误差距离5m）
                points = distance_dicts[vehicle_id]
                veh_pos = vehicle_info[tc.VAR_POSITION]
                veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                distance = calculate_distance(veh_lon, veh_lat, points[1], points[0])
                # 通过判断车辆车速 对未生效的车辆速度进行校正
                if vehicle_info[tc.VAR_SPEED] > float(points[2]) / 3.6:
                    acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                          float(points[2]) / 3.6, distance)
                    t = calc_time_to_reach2(float(points[2]) / 3.6, str(distance),
                                            vehicle_info[tc.VAR_SPEED], acceleration)
                    traci.vehicle.slowDown(vehicle_id, float(points[2]) / 3.6, t)
                    # traci.vehicle.setSpeed(vehicle_id, float(points[2]) / 3.6)
                if distance < 10:
                    # 说明车辆已经经过了这个点位，则需要判断下一个点位
                    number = 0
                    for i in range(len(speed_limit_points)):
                        lat = float(speed_limit_points[i]["lat"])
                        lon = float(speed_limit_points[i]["lon"])
                        if lat == points[0] and lon == points[1]:
                            number = i
                            break
                    # print(vehicles.items())
                    if number == len(speed_limit_points) - 1:
                        continue
                    else:
                        # 说明车辆还没有经过最后一个点位，则需要判断下一个点位
                        lat = float(speed_limit_points[number + 1]["lat"])
                        lon = float(speed_limit_points[number + 1]["lon"])
                        distance_dicts[vehicle_id] = [lat, lon, speed_speeds[number + 1]]
                        veh_pos = vehicle_info[tc.VAR_POSITION]
                        veh_lon, veh_lat = traci.simulation.convertGeo(veh_pos[0], veh_pos[1])
                        distance = calculate_distance(veh_lon, veh_lat, lon, lat)
                        acceleration = calculate_acceleration(float(vehicle_info[tc.VAR_SPEED]),
                                                              float(speed_speeds[number + 1]) / 3.6, distance)
                        t = calc_time_to_reach2(float(speed_speeds[number + 1]) / 3.6, str(distance),
                                                vehicle_info[tc.VAR_SPEED], acceleration)
                        traci.vehicle.slowDown(vehicle_id, float(speed_speeds[number + 1]) / 3.6, t)
                        # traci.vehicle.setSpeed(vehicle_id, float(HttpInterface.speeds[number+1]) / 3.6)
        # 获取数据在array里面，而不在distance_dicts的keys中的数据
        resultList = [a for a in distance_dicts.keys() if a not in array]
        for i in resultList:
            # todo 判断车辆是否在网
            distance_dicts.pop(i)
            traci.vehicle.setSpeed(i, -1)
    except Exception as e:
        print(e)


def in_bw_area(vehicles: dict, polygon: dict):
    """
    车辆进入恶劣天气区域范围后
    :param vehicles:
    :param polygon:
    :return:
    """
    global in_bw_area_veh_list
    for vehicle_id, vehicle_info in vehicles.items():
        speed = vehicle_info[tc.VAR_SPEED]
        speed_limit = polygon['speedLimit'] / 3.6
        if speed > speed_limit:
            if vehicle_info[tc.VAR_TYPE] != '0':
                if vehicle_id not in in_bw_area_veh_list:
                    in_bw_area_veh_list.append(vehicle_id)
                    accel = (speed_limit ** 2 - speed ** 2) / (2 * 35)
                    time = (speed_limit - speed) / accel
                    # print('veh_id:%s limit %s accel: %s time:%s' % (vehicle_id, speed_limit, accel, time))
                    traci.vehicle.setSpeed(vehicle_id, speed_limit)
            else:
                if 'v.0.0' not in in_bw_area_veh_list and 'v.0.3' not in in_bw_area_veh_list:
                    accel = (speed_limit ** 2 - speed ** 2) / (2 * 35)
                    time = (speed_limit - speed) / accel
                    # print('veh_id:%s limit %s accel: %s time:%s' % (vehicle_id, speed_limit, accel, time))
                    traci.vehicle.setAcceleration('v.0.0', accel - 0.2, time * 2)
                    # traci.vehicle.slowDown('v.0.0', speed_limit, time)
                    # plexe.set_cc_desired_speed('v.0.0', speed_limit)
                    # plexe.set_fixed_acceleration('v.0.0', True, accel)
                if vehicle_id not in in_bw_area_veh_list:
                    in_bw_area_veh_list.append(vehicle_id)


def out_bw_area(vehicles: list):
    """
    车辆离开恶劣天气区域
    :param vehicles:
    :return:
    """
    global in_bw_area_veh_list
    if len(in_bw_area_veh_list) > 0:
        for item in in_bw_area_veh_list[:]:
            if item not in vehicles:
                if 'v.0.' not in item:
                    try:
                        traci.vehicle.setSpeed(item, -1)
                    except TraCIException as t:
                        print("ERROR : 速度设置异常 -> ", str(t))
                in_bw_area_veh_list.remove(item)


# 限速区域
# 计算加速度减速度
# initial_velocity:初速度
# final_velocity:末速度
# displacement:位移
def calculate_acceleration(initial_velocity, final_velocity, displacement):
    acceleration = (final_velocity ** 2 - initial_velocity ** 2) / (2 * displacement)
    return acceleration


# 设置编队减速加速
plexe_flage = 0


def isPlexeReachTheSpecifiedPoint(dists, accelerations, distance, plexe):
    now = datetime.now()
    for i in range(len(dists) - 1):

        if distance <= dists[i] and i == 0:
            plexe.set_fixed_acceleration('v.0.0', True, accelerations[i])
            plexe.set_fixed_acceleration('v.0.1', True, accelerations[i])
            plexe.set_fixed_acceleration('v.0.2', True, accelerations[i])
            plexe.set_fixed_acceleration('v.0.3', True, accelerations[i])
        elif dists[i] <= distance and distance <= dists[i + 1]:
            # if (distance - dists[i]) < 1 or (dists[i + 1] - distance) < 1:
            plexe.set_fixed_acceleration('v.0.0', True, accelerations[i + 1])
            plexe.set_fixed_acceleration('v.0.1', True, accelerations[i + 1])
            plexe.set_fixed_acceleration('v.0.2', True, accelerations[i + 1])
            plexe.set_fixed_acceleration('v.0.3', True, accelerations[i + 1])
            traci.vehicle.setLaneChangeMode()


OPEN = 1
DOWN = 0
DELETE = -1
EMERGENCY_LANE_ALLOW_VCLASS = ['emergency', 'authority', 'army']
EXCLUSIVE_LANE_ALLOW_VCLASS = ['custom1']
STOP_EDGE = '-285.0.00'


# 简单寻路(单边)
def findEdgesAndTraffics(start_edge, end_edge, tl_ids, stop_edge=None):
    edge_list = list()
    if start_edge is None or end_edge is None:
        return edge_list
    edge_list.append(start_edge.getID())
    if start_edge.getID() == end_edge.getID():
        return edge_list
    traffic_list = list()
    temp_edge = start_edge
    count = 0
    while temp_edge.getID() != end_edge.getID() and count < 100:
        if count == 0 and temp_edge.getFromNode().getID() in tl_ids:
            traffic_list.append(temp_edge.getFromNode().getID())
        for k, v in temp_edge.getOutgoing().items():
            temp_edge = k
            if stop_edge is not None and temp_edge.getID() == stop_edge:
                break
            if temp_edge.getFromNode().getID() in tl_ids:
                traffic_list.append(temp_edge.getFromNode().getID())
            edge_list.append(k.getID())
        count += 1
    return edge_list, traffic_list


def controlEmergencyLane(action: int, edge, tl_ids):
    # 匹配道路

    # if HttpInterface.emergencyLaneSwitch is None or edge.getID() not in HttpInterface.emergencyLaneSwitch:
    #     return '未查询到对应应急车道'
    # if HttpInterface.emergencyLaneSwitch[edge.getID()] == action:
    #     return '未改变状态'
    lane_id = edge.getID() + '_0'
    if edge.getFromNode().getID() in tl_ids:
        state = traci.trafficlight.getRedYellowGreenState(edge.getFromNode().getID())
    if action == OPEN:
        # print("开启应急车道")
        aa = edge.getFromNode().getID()
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), 'G' + state[1:])
        traci.lane.setDisallowed(lane_id, [])
        # HttpInterface.emergencyLaneSwitch[edge.getID()] = action
    elif action == DOWN or action == -1:
        # print("关闭应急车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), 'r' + state[1:])
        traci.lane.setAllowed(lane_id, EMERGENCY_LANE_ALLOW_VCLASS)
        # HttpInterface.emergencyLaneSwitch[edge.getID()] = action
    else:
        # print("删除应急车道")
        if edge.getFromNode().getID() in tl_ids:
            traci.trafficlight.setRedYellowGreenState(edge.getFromNode().getID(), 'G' + state[1:])
        traci.lane.setDisallowed(lane_id, [])
        # del HttpInterface.emergencyLaneSwitch[edge.getFrom().getID()]


def bad_road_change_lane(plexe, bad_road: str, leave_road: str, bad_change_veh_set: set):
    """
    大于三车道坏路变道（左拐）
    :param plexe: 变道模型类
    :param bad_road: 坏路junction标记
    :param leave_road: 离开坏路的junction标记
    :param change_veh_records: 被set_fixed_lane的车辆集合，离开时disable_fixed_lane
    :return:
    """
    bad_result = traci.junction.getContextSubscriptionResults(bad_road)
    if len(bad_result) > 0:
        for veh_id, veh_info in dict(bad_result).items():
            if veh_info[tc.VAR_TYPE] != 'vtypeauto' and veh_info[tc.VAR_LANE_INDEX] < 2:
                print("左拐")
                left_followers = traci.vehicle.getLeftFollowers(veh_id)
                if len(left_followers) > 0:
                    print("左后方有车")
                    if abs(left_followers[0][1]) < 7:
                        if left_followers[0][0] not in bad_change_veh_set:
                            plexe.set_fixed_lane(left_followers[0][0], int(veh_info[tc.VAR_LANE_INDEX]) + 2,
                                                 False)
                            bad_change_veh_set.add(left_followers[0][0])
                    else:
                        plexe.set_fixed_lane(veh_id, int(veh_info[tc.VAR_LANE_INDEX]) + 1, False)
                        bad_change_veh_set.add(veh_id)
                else:
                    plexe.set_fixed_lane(veh_id, int(veh_info[tc.VAR_LANE_INDEX]) + 1, False)
                    bad_change_veh_set.add(veh_id)

    leave_result = traci.junction.getContextSubscriptionResults(leave_road)
    if len(leave_result) > 0:
        for veh_id in list(bad_change_veh_set):
            if veh_id in leave_result:
                plexe.disable_fixed_lane(veh_id)
                bad_change_veh_set.remove(veh_id)


def findNewEdge(oldEdge):
    if INTERNAL_SIGN not in str(oldEdge):
        return oldEdge
    for k, _ in oldEdge.getOutgoing().items():
        return k


"""
 获取中心坐标
 :param geolocations: [(lat, lon)]
"""


def center_geolocation(geolocations: list):
    x = 0
    y = 0
    z = 0
    length = len(geolocations)
    for lat, lon in geolocations:
        lon = radians(float(lon))
        lat = radians(float(lat))
        x += cos(lat) * cos(lon)
        y += cos(lat) * sin(lon)
        z += sin(lat)
    x = float(x / length)
    y = float(y / length)
    z = float(z / length)
    return (degrees(atan2(y, x)), degrees(atan2(z, sqrt(x * x + y * y))))


def get_time_to_reach_target(current_speed: str, current_acceleration: str, target_speed: str,
                             distance_to_target: str) -> float:
    # 将输入字符串转换为浮点数
    current_speed = float(current_speed)
    current_acceleration = float(current_acceleration)
    target_speed = float(target_speed)
    distance_to_target = float(distance_to_target)

    # 计算到达指定位置所需的时间
    time = (target_speed - current_speed) / current_acceleration + math.sqrt(
        2 * distance_to_target / current_acceleration + (target_speed - current_speed) ** 2 / current_acceleration ** 2)
    return time


def calc_time_to_reach2(target_speed, target_distance, current_speed, current_acceleration):
    # 将字符串类型的参数转换为浮点数类型
    v = float(target_speed)
    d = float(target_distance)
    v0 = float(current_speed)
    a = float(current_acceleration)
    t = (v - v0) / a + (d - (v ** 2 - v0 ** 2) / (2 * a)) / v
    return t


def calc_time_to_reach(target_speed, target_distance, current_speed, current_acceleration):
    # 将字符串类型的参数转换为浮点数类型
    target_speed = float(target_speed)
    target_distance = float(target_distance)
    current_speed = float(current_speed)
    current_acceleration = float(current_acceleration)

    # 如果加速度为负数，则表示减速
    if current_acceleration < 0:
        current_acceleration = abs(current_acceleration)

    # 计算加速度对应的时间和距离
    t_accel = (target_speed - current_speed) / current_acceleration
    d_accel = current_speed * t_accel + 0.5 * current_acceleration * t_accel ** 2

    # 如果需要的距离小于加速阶段行驶的距离，则只需考虑加速阶段的时间
    if target_distance <= d_accel:
        t_total = 2 * math.sqrt(target_distance / current_acceleration) + (
                target_speed - current_speed) / current_acceleration
    else:
        # 否则需要先加速到目标速度，再匀速行驶，最后减速到目标速度
        t_accel_to_target = (target_speed - current_speed) / current_acceleration
        d_accel_to_target = current_speed * t_accel_to_target + 0.5 * current_acceleration * t_accel_to_target ** 2

        d_decel_to_target = target_distance - d_accel_to_target
        t_decel_to_target = math.sqrt(2 * d_decel_to_target / current_acceleration)

        t_total = t_accel_to_target + t_decel_to_target + (d_decel_to_target / target_speed)

    return t_total


DEFAULT_CONFIG_FILE_PATH = './config/longshan.sumocfg'


def start_sumo(config_file=DEFAULT_CONFIG_FILE_PATH, already_running=False, custom_arg=None, gui=True):
    """
    Starts or restarts sumo with the given configuration file
    :param config_file: sumo configuration file
    :param already_running: if set to true then the command simply reloads
    the given config file, otherwise sumo is started from scratch
    :param custom_arg: label param
    :param gui: start GUI or not
    """
    # 检测是否已经添加环境变量
    if 'SUMO_HOME' in os.environ:
        tools = os.path.join(os.environ['SUMO_HOME'], 'tools')
        sys.path.append(tools)
    else:
        sys.exit("please declare environment variable 'SUMO_HOME'")
    if custom_arg is None:
        custom_arg = build_custom_arg(delay=10, config_file=config_file)
    sumo_cmd = [sumolib.checkBinary('sumo-gui' if gui else 'sumo')]
    # custom_arg.append(config_file)
    if already_running:
        traci.load(custom_arg)
    else:
        sumo_cmd.extend(custom_arg)
        print(sumo_cmd)
        traci.start(sumo_cmd)

        if gui:
            traci.gui.setSchema("View #0", "real world")


# xy转换经纬度
def xyConversionlatAndLon(sumo_net, pos):
    rule1 = sumo_net.getGeoProj()
    offset = sumo_net.getLocationOffset()

    longitude, latitude = rule1(pos[0] - offset[0], pos[1] - offset[1], inverse=True)
    return longitude, latitude


# 经纬度转换xy
def latAndLonConversionXY(sumo_net, pos):
    rule1 = sumo_net.getGeoProj()
    offset = sumo_net.getLocationOffset()

    x1, y1 = rule1(pos['lon'], pos['lat'])

    x = x1 + offset[0]
    y = y1 + offset[1]
    return x, y


def build_custom_arg(delay=100, step_length=0.1, lane_change_duration=3,
                     config_file=DEFAULT_CONFIG_FILE_PATH):
    return [
        '--delay', str(delay),
        '--step-length', str(step_length),
        '--collision.check-junctions',
        '-c', config_file,
        '--threads', '64',
        '--mesosim', 'false',
        '--no-warnings','true',
        '--max-depart-delay','0.5'

    ]


# 匝道管控
# 统计最近一分钟通过匝道红绿灯车辆数量
vehicles = set()


def addRampControl(vehicleids, rampControlNum: int):
    global vehicles
    for vehicleid in vehicleids:
        try:
            lane_id = traci.vehicle.getLaneID(vehicleid)
            if "694" in lane_id:
                vehicles.add(vehicleid)
        except TraCIException as e:
            print(str(e))
    times = traci.simulation.getTime()

    if times % 60 == 0:
        if len(vehicles) > rampControlNum:
            traci.trafficlight.setRedYellowGreenState("gneJ0", "r")
            vehicles.clear()
        else:
            traci.trafficlight.setRedYellowGreenState("gneJ0", "G")


def getFreeRoute(sumo_net_file: str, routeid: str, vehiclePoints: list, result: list):
    if len(vehiclePoints) == 0:
        # vehicleNum=50
        vehiclePoints = '[]'
        # vehiclePoints='[{"lat":34.372449,"lon":108.892912},{"lat":34.375380,"lon":108.899382}]'
        result = [{"arrive": [],
                   "depart": {"autonomousRatio": 0, "busRatio": 0, "count": 0, "point": [], "simplePassengerRatio": 0},
                   "id": "",
                   "route": [],
                   "type": 1}
                  ]
    sumo_net = sumolib.net.readNet(sumo_net_file, withInternal=True, withLatestPrograms=True)
    # vehiclePointsobj = json.loads(vehiclePoints)
    # for vehiclePoint in vehiclePoints:
    # lat = float(vehiclePoint["lat"])
    lat = vehiclePoints[1]
    # lon = float(vehiclePoint["lon"])
    lon = vehiclePoints[0]
    x, y = sumo_net.convertLonLat2XY(lon, lat)
    sumo_cfg_file = "D:\pythonproject\changansumo\CADXWCFZ_sumo\webgl\长安大学_openDrive_v0.sumocfg"
    # start_sumo(sumo_cfg_file, False, gui=False)
    traci.start(cmd=['sumo',
                     '-c', sumo_cfg_file,
                     '--step-length', str(0.1),
                     # '--lateral-resolution', '0.25',
                     '--collision.check-junctions',
                     # '--fcd-output', out_file,
                     # "--fcd-output.geo",
                     '--lanechange.duration', '1']
                )
    # x,y=sumo_utils.latAndLonConversionXY(sumo_net,(lon, lat))
    startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon, lat, True)
    traci.close()
    # startedgeID='-291.0.00'
    single_edge = sumo_net.getEdge(startedgeID)
    if single_edge is not None:
        route = [startedgeID]
        count = 0
        while single_edge is not None:
            edge_Bound = single_edge.getOutgoing()
            edgeids = list()
            for k, v in edge_Bound.items():
                edgeids.append(k.getID())
            if edgeids is not None and len(edgeids) > 0:
                try:
                    id = random.choice(edgeids)
                    if True:
                        route.append(id)
                    else:
                        logging.info("Road marking problem1")
                        errorMsg = "标注错误，请重新标注"
                    single_edge = sumo_net.getEdge(id)
                except ValueError:
                    continue
                # break
            count += 1
            if len(route) > 1000 or count > 200 or '-547.0.00' in route:
                print(route)
                break
        # routeid = str(uuid.uuid1())
        routes = list()
        for edge_id in route:
            if len(str(edge_id).strip()) == 0:
                continue
            edge = sumo_net.getEdge(edge_id)
            if edge_id == '-301.0.00' or edge_id == '-302.0.00' or edge_id == '-276.0.00' or edge_id == '-266.0.00' or edge_id == '-268.0.00' or edge_id == '-260.0.00' or edge_id == '-264.0.00' or edge_id == '-560.0.00' or edge_id == '-549.0.00':
                shape = edge.getRawShape3D()
                for sh in shape:
                    point = sumo_net.convertXY2LonLat(sh[0], sh[1])
                    routes.append(point)
            else:
                shape = edge.getRawShape3D()
                start_coord = shape[0]  # x, y, z of the start of the edge
                center_coord = shape[len(shape) // 2]  # x, y, z of the center of the edge
                end_coord = shape[-1]  # x, y, z of the end of the edge
                # point = sumo_net.convertXY2LonLat(shape[0][0], shape[0][1])
                start_coord_point = sumo_net.convertXY2LonLat(start_coord[0], start_coord[1])
                center_coord_point = sumo_net.convertXY2LonLat(center_coord[0], center_coord[1])
                end_coord_point = sumo_net.convertXY2LonLat(end_coord[0], end_coord[1])
                routes.append(start_coord_point)
                routes.append(center_coord_point)
                routes.append(end_coord_point)
            # if len(routes) > 0:

        result[0]['route'] = routes
        result[0]['id'] = routeid
        # result.append(RouteDetail(routeid, routes, v_count=0, a_type=2, simple_passenger_ratio=0.0, bus_ratio=0.0,
        #                 autonomous_ratio=0.0).to_dict())

        # traci.route.add(routeid, route)




    else:
        logging.info("Road marking problem1")
        errorMsg = "标注错误，请重新标注"
    return result


def get_surrounding_vehicles(tag_vehicle_id, global_vehicles, radius):
    surrounding_vehicles = []
    x, y = traci.vehicle.getPosition(tag_vehicle_id)
    for vehicle_id, veh_info in dict(global_vehicles).items():
        if vehicle_id == vehicle_id:
            # 排除要查询的车辆本身
            continue
        v_x, v_y = veh_info[tc.VAR_POSITION]
        distance = ((v_x - x) ** 2 + (v_y - y) ** 2) ** 0.5
        if distance <= radius:
            surrounding_vehicles.append(vehicle_id)
    return surrounding_vehicles
